#include "includes.h" 

char Pixel[128]={0};

int coordinate,last_coordinate;
int Right=84,Left=44,last_left=44,last_right=84;
int point_flag=0,point,r_flag=1,c_flag=0,cc_flag=0; 
int point_array[10],change_array[10],chang_error;

unsigned int average,blacklnum,whitelnum,blackl_aver,whitel_aver,error;
unsigned long sum,blacklsum,whitelsum;
extern int  Averagevalue ;
    
unsigned int ReadATD(unsigned char Ch)   //AD转换
{
   unsigned int AD_data;
   ATD0CTL5_Cx = Ch;
   while(!ATD0STAT0_SCF);
   AD_data = ATD0DR0;
   return(AD_data); 
 }

void Pixel_Get(void)        // CCD初始化并采集数据
{
  unsigned int temp_int;
  byte i=0; 
  TSL_SI=0;     //下降沿
  TSL_CLK=0;     //下降沿
  Delay_us(1);  
     
  TSL_SI=1;     //上升沿
  Delay_us(1); 
  TSL_CLK=1;    //上升沿
  Delay_us(1);     //合理延时
  
  TSL_SI=0;     //下降沿
  Delay_us(1);      //合理延时 
  Delay_us(10);    //延时10us为第一个准备
  temp_int = ReadATD(0);
  Pixel[0]=(byte)(temp_int>>4);  //采集第一个点
  
  TSL_CLK=0;     //下降沿      
  for(i=1;i<128;i++)     //采集第2-128点
  {    
    Delay_us(1);      //合理延时
    TSL_CLK=1;     //上升沿 
    Delay_us(1);      //合理延时
    temp_int = ReadATD(0);
    Pixel[i]=(byte)(temp_int>>4);  //AD采集
    TSL_CLK=0;//下降沿    
  }
  //发送第129个CLK
  Delay_us(1); 
  TSL_CLK=1;    //上升沿
  Delay_us(1);     //合理延时
  TSL_CLK=0;     //下降沿
  Delay_us(1);     //合理延时
} 

unsigned int a_b(int a,int b)   //相减取绝对值
{           
  int x;
  x=a-b;
  if(x>=0)
   return x;
  else 
   return -x;
}

//* ** *** **** ***** ****CCD凹槽读取位置判定***** ***** **** *** ** *//
void  Position_calculate(void) 
{
  int i,j=1,temp,p=0,k,m=0,b_flag=0;
  static int s_flag=0,last_r_flag=1,y_flag=0,o=0,oo=0;
  
  
  sum=0,blacklsum=0,blacklnum=0,point_flag=0;
  
  Pixel_Get(); 
  c_flag=1; cc_flag=1;
  
  
   
  for(i=0;i<10;i++)  
  {
   point_array[i]=0;
   change_array[i]=0;
  }

  for(i=20;i<100;i++) 
  {
    sum+=Pixel[i];            
  }
  average=(int)(sum/80);

  for(i=20;i<108;i++) 
  {
    if(Pixel[i]<average)
    {
     blacklsum+=Pixel[i];
     blacklnum++;
    }
  }
  blackl_aver=(unsigned int)(blacklsum/blacklnum);
  whitelsum=sum-blacklsum;
  whitel_aver=(unsigned int)(whitelsum/(88-blacklnum));
  error=whitel_aver-blackl_aver;
  
  for(i=25;i<100;i++)                          //  Trough   recognition
  {
   if((Pixel[i]-Pixel[i+2]>=2)&&((Pixel[i+4]-Pixel[i+2]>=2)||(Pixel[i+4]-Pixel[i+3]>=2))) 
    {
     temp=Pixel[i]; 
     for(k=i;k<i+5;k++)  
     {  
       if(Pixel[k]<temp)
       {          
         temp=Pixel[k];
         m=k; 
       } 
     }
     if(Pixel[m]<average) 
     {  
       point=m;
       point_array[j]=point;
       point_flag++;
       i=i+4;
       j++;
     }
    } 
  } 
  
  
    
//* ** *** **** ***** **** *** ** * * ** *** **** ***** **** *** ** *//
 if(point_flag>0)                               //  Trough filled
 {  
   if(point_flag==1)
   {
     b_flag=1;
     if(point_array[1]<60)
     point_array[0]=point_array[1]-50;
     else 
     {    
       o++;
       if(o>=2)
       point_array[0]=0;
       else
       point_array[0]=point_array[1]-50;
       
     } 
     point_flag++;
     
     if(point_array[point_flag-1]>=60)
     point_array[point_flag]=point_array[point_flag-1]+50; 
     else 
     {
       oo++;
       if(oo>=2)
       point_array[point_flag]=130;
       else
       point_array[point_flag]=point_array[point_flag-1]+50; 
      
     }
     point_flag++;  
    }

  if(!b_flag) 
  {  
    o=0;
    oo=0;
   
    point_array[0]=point_array[1]-50;
    point_flag++;
    point_array[point_flag]=point_array[point_flag-1]+50; 
    point_flag++;
  }
  
  chang_error=(int)((last_left+last_right)/2);

   for(i=0;i<point_flag;i++)                    //  The nearest black line
   { 
     change_array[i]=a_b(point_array[i],chang_error);
   }
    
   temp=change_array[0];                        //  The nearest black line
   for(i=1;i<point_flag;i++)  
   {  
     if(change_array[i]<temp)
     {          
       temp=change_array[i]; 
       p=i;
     } 
   }
    
   if(change_array[p]>24)                       // To the nearest black line to identify true and false
   {
     Right=last_right; 
     Left=last_left; 
   } 
   else                                         // the black lines to the left or right
   {  
     if(a_b(point_array[p],last_left)>=a_b(point_array[p],last_right))                
     {
       Right=point_array[p]; 
       if(point_array[p]-point_array[p-1]>20)
       Left=point_array[p-1];
       else
       Left=point_array[p-2];
       if(p-2<0)
       Left=point_array[0];     
     }                                      
     else 
     {  
       Left=point_array[p];
       if(point_array[p+1]-point_array[p]>20)
       Right=point_array[p+1];
       else
       Right=point_array[p+2];
       if(p+2>point_flag-1)
       Right=point_array[point_flag-1];         
     }  
   
     if(a_b(last_left,Left)>50||a_b(last_right,Right)>50) 
      {
        Right=last_right; 
        Left=last_left; 
      }  
     
      if(Right-Left<20)      //滤除错误相同的坐标
      {
        Right=last_right; 
        Left=last_left; 
      }               
   }     
   coordinate=128-(Left+Right);   
  }   
  
  
  if(point_flag==0)      //丢两条线，判十字或者全打出去丢线
  {
     if(abs(Averagevalue)<20) 
     { 
        s_flag++;  
        Left=44;
        Right=84;
        coordinate=128-(Left+Right);  
     } 
     else
     { 
        if(((Right>45&&Right<75)||(Left>45&&Left<75)))         
        {
           s_flag++; 
           Left=44;
           Right=84;
           coordinate=128-(Left+Right);   
        } 
        else                             //if(Right<35||Left>80)   //冲出赛道
        {  
          Right=last_right; 
          Left=last_left;
          coordinate=(int)(2*last_coordinate);
        }
     }
      
  }
  if(s_flag>=2)            //处理十字道内部
  {
    coordinate=0; 
    if(point_flag>=3) 
    {
   
      if(point_flag==3)  
      {
       if( point_array[1]<64)
       { 
         Left= point_array[1];
         Right=Left+50;
       } 
       else
       {
         Right= point_array[1];
         Left =Right-50;
       }
      }  
  
      if(point_flag>=4&&(point_array[2]-point_array[1])>35) 
      {
        Left=point_array[1];
        Right=point_array[2];
        s_flag=0; 
      }  
      if(point_flag>=5&&(point_array[3]-point_array[2])>35) 
      {
        Left= point_array[2];
        Right=point_array[3];
        s_flag=0;  
      }
      coordinate=128-(Left+Right);
    }
  }
    
  /* 
  if((last_left>70||last_right<60)&&point_flag>0)
  coordinate=(int)(1.2*(128-(Left+Right)));
  

  if(Right<35||Left>80)     // 偏左或偏右
  {  
    r_flag=2;
  } 
  else
  {
     r_flag=1;  
  } 
 

  if(point_flag==0)
  {    
    Right=last_right; 
    Left=last_left;
   
    if( r_flag==1&&last_r_flag==1) 
    {
      Left=44;
      Right=84;   
      s_flag++;     
      coordinate=(int)(0.1*last_coordinate); 
    }
   
    else
    {    
      coordinate=(int)(2*last_coordinate); 
    }
  }
  
  if(s_flag>=3) 
  {
    coordinate=0; 
    if(point_flag>=3) 
    {
   
      if(point_flag==3)  
      {
       if( point_array[1]<64)
       { 
         Left= point_array[1];
         Right=Left+50;
       } 
       else
       {
         Right= point_array[1];
         Left =Right-50;
       }
      }  
  
      if(point_flag==4&&(point_array[2]-point_array[1])>35) 
      {
        Left=point_array[1];
        Right=point_array[2];
        s_flag=0; 
      } 
      else if(point_flag>=5&&(point_array[2]-point_array[1])>35) 
      {
        Left=point_array[1];
        Right=point_array[2];
        s_flag=0;    
      } 
      else if(point_flag>=5&&(point_array[3]-point_array[2])>35) 
      {
        Left= point_array[2];
        Right=point_array[3];
        s_flag=0;  
      }
      coordinate=128-(Left+Right);
 
    }
  }     */
  
  
  if(coordinate>200)
  coordinate=200;
  if(coordinate<-200)
  coordinate=-200;
  
 // last_r_flag=r_flag; 
  last_left=Left;
  last_right=Right;
  last_coordinate= coordinate; 
 
   
}